﻿#include "postestwidget.h"

QPosTestWidget::QPosTestWidget(QWidget *parent) : QWidget(parent)
{

    _i32SaveFrameCnt = 0;
    _bStartSaveData = false;
    _bStartTest = false;
    _i32FrameCnt = 0;
    _i32AAFrameCnt = 0;


    InitUI();

    QString sInfo;
    sInfo = sInfo + QString("当前KP:0.0\n");
    sInfo = sInfo + QString("当前KI:0.0\n");
    sInfo = sInfo + QString("当前KD:0.0\n");

    _pShowDataLabel->setText(sInfo);


    _pPlotDataTimer = new QTimer(this);
    connect(_pPlotDataTimer,&QTimer::timeout,this,[=](){plotPosData();plotSpeedData();});

    void (QButtonGroup::*fun)(int) = &QButtonGroup::buttonClicked;
    connect(_pAxisBtnGroup,fun,this,&QPosTestWidget::onAxiBtnClick);

    void (QComboBox::*fun1)(int) = &QComboBox::currentIndexChanged;
    connect(_pTestTypeComBox,fun1,this,[=](int index){_pStackedWidget->setCurrentIndex(index);});


    connect(_pStartBtn,&QPushButton::clicked,this,&QPosTestWidget::onStartBtnClicked);
    connect(_pStopBtn,&QPushButton::clicked,this,&QPosTestWidget::onStopBtnClicked);
    connect(_pSetParamBtn,&QPushButton::clicked,this,&QPosTestWidget::onSetParamBtnClicked);
    connect(_pModifyParamBtn,&QPushButton::clicked,this,&QPosTestWidget::onModifyParamBtnClicked);

    connect(_pEnabelBtn,&QPushButton::clicked,this,&QPosTestWidget::onEnabelBtnClicked);

    connect(_pSaveDataCheckBox,&QCheckBox::clicked,this,&QPosTestWidget::onSaveDataBtnClick);

    //默认选中方位轴
    _i32AixsNo = 0;
    _pAxisBtnGroup->button(0x0)->setChecked(true);
}

/**
* @brief
*
**/
void QPosTestWidget::InitUI()
{


    _pLogWidget = new QLogWidget(this);

    _pPosPlot = new QCustomPlot(this);
    _pSpeedPlot = new QCustomPlot(this);

//    setPosPlotStyle(_pPosPlot,"时间(ms)","位置(度)");
    setPlotStyle(_pSpeedPlot,"时间(ms)","速度(度/S)");
    setPlotStyle(_pPosPlot,"时间(ms)","位置(度)");


    _pParamWidget = createParamWidget();


    QGridLayout* playout = new QGridLayout(this);
    playout->setSpacing(0);

    playout->addWidget(_pSpeedPlot,0,0,1,1);
    playout->addWidget(_pPosPlot,1,0,1,1);
    playout->addWidget(_pParamWidget,0,1,3,1);
    playout->addWidget(_pLogWidget,2,0,1,1);

    playout->setColumnStretch(0,4);
    playout->setColumnStretch(1,1);

    playout->setRowStretch(0,2);
    playout->setRowStretch(1,2);
    playout->setRowStretch(2,1);

}
/**
* @brief
*
**/
QWidget* QPosTestWidget::createAxisWidget()
{
   QStringList sAxiNamelist;
   sAxiNamelist<<"方位轴"<<"俯仰轴";
   _pAxisBtnGroup = new QButtonGroup(this);


  QLabel* plabel = new QLabel("轴选择:",this);
  plabel->setStyleSheet("QLabel{color:rgb(0,0,255);font-size:20px;font-family: 'Microsoft YaHei';}");

  QWidget* pwidget = new QWidget(this);

  QGridLayout* playout = new QGridLayout(pwidget);

   int nRow = 0;

   playout->addWidget(plabel,nRow,0,1,1);

   for(int i = 0; i < sAxiNamelist.size();i++)
   {
       QPushButton* pButton = new QPushButton(sAxiNamelist[i],this);
       pButton->setCheckable(true);
       setButtonStyleSheet(pButton);
       _pAxisBtnGroup->addButton(pButton,i);
       playout->addWidget(pButton,nRow,1+i,1,1);
   }

   return pwidget;

}


/**
* @brief
*
**/
QWidget* QPosTestWidget::createParamWidget()
{
    QWidget* pwidget = new QWidget(this);
    QUIQss::setWidgetStyleSheet(pwidget);


    _pAxisSelectWidget = createAxisWidget();
    _pAxisSelectWidget->setHidden(true);


    _pSaveDataCheckBox = new QCheckBox("保存实时数据",this);



    _pSetParamBtn = new QPushButton("调试参数",this);
    _pModifyParamBtn = new QPushButton("注入参数",this);
    _pEnabelBtn = new QPushButton("开使能",this);


    QLabel* plabel1 = new QLabel("控制器比例KP:",this);
    QLabel* plabel2 = new QLabel("控制器比例KI(Hz):",this);
    QLabel* plabel3 = new QLabel("控制器比例KD:",this);

    _pKPEdit = new QLineEdit("0",this);
    _pKIEdit = new QLineEdit("0",this);
    _pKDEdit = new QLineEdit("0",this);


    _pShowDataLabel = new QLabel(this);
    _pShowDataLabel->setMinimumHeight(100);
    _pShowDataLabel->setStyleSheet("color:rgb(0,0,255);font-size:20px;font-family: 'Microsoft YaHei';");


    _pShowPosInfoLabel = new QLabel(this);
    _pShowPosInfoLabel->setMinimumHeight(100);
    _pShowPosInfoLabel->setStyleSheet("color:rgb(0,0,255);font-size:20px;font-family: 'Microsoft YaHei';");






    int nRow = 0;
    QGridLayout* playout = new QGridLayout(pwidget);


    playout->addWidget(_pAxisSelectWidget,nRow,0,1,4);


    nRow++;
    playout->addWidget(plabel1,nRow,0,1,1);
    playout->addWidget(_pKPEdit,nRow,1,1,3);
    nRow++;
    playout->addWidget(plabel2,nRow,0,1,1);
    playout->addWidget(_pKIEdit,nRow,1,1,3);
    nRow++;
    playout->addWidget(plabel3,nRow,0,1,1);
    playout->addWidget(_pKDEdit,nRow,1,1,3);
    nRow++;
    playout->addWidget(_pSetParamBtn,nRow,0,1,1);
    playout->addWidget(_pModifyParamBtn,nRow,1,1,3);
    nRow++;
    playout->addWidget(_pEnabelBtn,nRow,0,1,1);
    nRow++;
    playout->addWidget(_pShowDataLabel,nRow,0,1,4);
    nRow++;
    playout->addWidget(createGroudTestWidget(),nRow,0,1,4);
    nRow++;

    playout->addWidget(_pSaveDataCheckBox,nRow,0,1,4);
    nRow++;

    playout->addWidget(_pShowPosInfoLabel,nRow,0,1,4);
    nRow++;

    playout->addItem(new QSpacerItem(10,350,QSizePolicy::Minimum,QSizePolicy::Maximum),nRow,3,1,1);

    return pwidget;
}
/**
* @brief
*
**/
QWidget* QPosTestWidget::createSignalWidget()
{
    QWidget* pwidget = new QWidget();

    _pSignalTypeComBox = new QComboBox(this);
    _pSignalTypeComBox->addItem("方波");
    _pSignalTypeComBox->addItem("正弦波");
    _pSignalTypeComBox->setMinimumHeight(30);

    _pFreqEdit = new QLineEdit("0",this);
    _pFreqEdit->setToolTip("频率范围:[0,20]Hz");

    _pAmpEdit = new QLineEdit("0",this);
    _pAmpEdit->setToolTip("角度:[0,100]度");
    QUIQss::setTxtQss(_pFreqEdit, "#DCE4EC", "#1ABC9C");
    QUIQss::setTxtQss(_pAmpEdit, "#DCE4EC", "#1ABC9C");


    QLabel* plabel = new QLabel("信号类型:",this);
    QLabel* plabel3 = new QLabel("信号频率:",this);
    QLabel* plabel4 = new QLabel("角度(°):",this);

    int nRow = 0;
    QGridLayout* playout = new QGridLayout(pwidget);
    playout->setSpacing(0);
    playout->setMargin(0);

    playout->addWidget(plabel,nRow,0,1,1);
    playout->addWidget(_pSignalTypeComBox,nRow,1,1,3);
    nRow++;
    playout->addWidget(plabel3,nRow,0,1,1);
    playout->addWidget(_pFreqEdit,nRow,1,1,3);
    nRow++;
    playout->addWidget(plabel4,nRow,0,1,1);
    playout->addWidget(_pAmpEdit,nRow,1,1,3);
    nRow++;

    return pwidget;

}
/**
* @brief
*
**/
QWidget* QPosTestWidget::createPosSetwidget()
{
    QWidget* pwidget = new QWidget();

    QLabel* plabel = new QLabel("给定位置:",this);
    _pSetPosEdit = new QLineEdit("0");
    QUIQss::setTxtQss(_pSetPosEdit, "#DCE4EC", "#1ABC9C");

    QLabel* plabel1 = new QLabel("前馈设置:",this);
    _pFrontFeedComBox = new QComboBox(this);
    _pFrontFeedComBox->setMinimumHeight(40);
    _pFrontFeedComBox->addItem("加入前馈");
    _pFrontFeedComBox->addItem("无前馈");


    int nRow = 0;
    QGridLayout* playout = new QGridLayout(pwidget);
    playout->setSpacing(0);
    playout->setMargin(0);

    playout->addWidget(plabel,nRow,0,1,1);
    playout->addWidget(_pSetPosEdit,nRow,1,1,3);
    nRow++;
    playout->addWidget(plabel1,nRow,0,1,1);
    playout->addWidget(_pFrontFeedComBox,nRow,1,1,3);
    return pwidget;

}
QWidget* QPosTestWidget::createGroudTestWidget()
{

    QWidget* pwidget = new QWidget();

   // QLabel* plabel3 = new QLabel("测试类型:",this);
    _pTestTypeComBox = new QComboBox(this);
    _pTestTypeComBox->addItem("给定位置测试");
//    _pTestTypeComBox->addItem("周期信号测试");
    _pTestTypeComBox->setMinimumHeight(30);
    _pTestTypeComBox->setHidden(true);

    _pStackedWidget = new QStackedWidget(this);

    QWidget* pwidget0 = createPosSetwidget();
    QWidget* pwidget1 = createSignalWidget();

    _pStackedWidget->addWidget(pwidget0);
    _pStackedWidget->addWidget(pwidget1);

    _pStartBtn = new QPushButton("开始测试",this);
    _pStopBtn = new QPushButton("停止测试",this);


    int nRow = 0;
    QGridLayout* playout = new QGridLayout(pwidget);
    playout->setSpacing(0);
    playout->setMargin(0);

//    playout->addWidget(plabel3,nRow,0,1,1);
//    playout->addWidget(_pTestTypeComBox,nRow,1,1,3);
//    nRow++;
    playout->addWidget(_pStackedWidget,nRow,0,1,4);
    nRow++;
    playout->addWidget(_pStartBtn,nRow,2,1,1);
    playout->addWidget(_pStopBtn,nRow,3,1,1);

    return pwidget;
}

/**
* @brief
*
**/
void QPosTestWidget::setPlotStyle(QCustomPlot* pPlot,QString xName,QString yName)
{
    pPlot->xAxis->setLabel(xName);
    pPlot->yAxis->setLabel(yName);

//    pPlot->xAxis->setRange(0, 1024);//设置范围
//    pPlot->yAxis->setRange(0, 100);

    pPlot->setInteractions(QCP::iRangeDrag | QCP::iRangeZoom /*| QCP::iSelectPlottables*/ | QCP::iMultiSelect);

    pPlot->addGraph();//增加通道
    pPlot->addGraph();//增加通道

    pPlot->graph(0)->setName("设定值");
    pPlot->graph(1)->setName("反馈值");


    QPen pen;
    pen.setColor(QColor(0,0,255));
    pen.setWidth(1);
    pPlot->graph(0)->setPen(pen);


    pen.setColor(QColor(255,0,0));
    pPlot->graph(1)->setPen(pen);


    pPlot->legend->setVisible(true);
    pPlot->legend->setFont(QFont("Helvetica",9));

}

/**
* @brief
*
**/
void QPosTestWidget::setPosPlotStyle(QCustomPlot* pPlot,QString xName,QString yName)
{
    pPlot->xAxis->setLabel(xName);
    pPlot->yAxis->setLabel(yName);


    pPlot->setInteractions(QCP::iRangeDrag | QCP::iRangeZoom /*| QCP::iSelectPlottables*/ | QCP::iMultiSelect);

    pPlot->addGraph();//增加通道

    pPlot->graph(0)->setName("反馈值");


    QPen pen;
    pen.setColor(QColor(255,0,0));
    pPlot->graph(0)->setPen(pen);


    pPlot->legend->setVisible(true);
    pPlot->legend->setFont(QFont("Helvetica",9));

}

/**
* @brief
*
**/
void QPosTestWidget::onAxiBtnClick(int id)
{
    _i32AixsNo = id;
}

/**
* @brief
*
**/
void QPosTestWidget::onStartBtnClicked()
{

//    _xposdatalist.clear();
//    _yposlist[0].clear();
//    _yposlist[1].clear();
//    _yspeedlist[0].clear();
//    _yspeedlist[1].clear();
//    _xspeeddatalist.clear();

    if(_xposdatalist.size() == 0)
    {
        _pPosPlot->graph(0)->data().data()->clear();//清除数据
        _pPosPlot->graph(1)->data().data()->clear();//清除数据
        _pSpeedPlot->graph(0)->data().data()->clear();//清除数据
        _pSpeedPlot->graph(1)->data().data()->clear();//清除数据
    }
    //给定模式
    if(_pTestTypeComBox->currentIndex() == 0x00)
    {
        float l_lfPos = _pSetPosEdit->text().toFloat();//信号类型
        float lfCoeff = 131072/360.0;

        //前馈状态
        uchar l_frontdata = _pFrontFeedComBox->currentIndex() == 0x0?0xAA:0x00;

        fourchar l_currentvalue;
        l_currentvalue.i32data = l_lfPos*lfCoeff;

        //前馈使能
        l_currentvalue.data[3] = l_frontdata;//

        emit signal_startTest(_i32AixsNo,0x2A,l_currentvalue.i32data,0x0);
    }

    if(_pTestTypeComBox->currentIndex() == 0x01)
    {
        int l_i32SigalType = _pSignalTypeComBox->currentIndex()+1;//信号类型
        int l_i32Freq = _pFreqEdit->text().toFloat()*10;//频率
        short l_i16Amp = _pAmpEdit->text().toFloat()*10;

        fourchar l_data;
        l_data.i32data = 0x0;
        l_data.data[3] = (l_i32SigalType&0xff);
        l_data.data[2] = (l_i32Freq&0xff);
        l_data.data[1] = (l_i16Amp>>8);
        l_data.data[0] = (l_i16Amp&0xff);

        emit signal_startTest(_i32AixsNo,0x7D,l_data.ui32data,0);
    }


    _bStartTest = true;

    _pPlotDataTimer->start(500);

    _pLogWidget->onShowlog("开始测试..");

}
/**
* @brief
*
**/
void QPosTestWidget::onStopBtnClicked()
{
    _pPlotDataTimer->stop();
    emit signal_startTest(_i32AixsNo,0x1B,0,0);//关使能
    _bStartTest = false;
    _pLogWidget->onShowlog("停止测试！",true);

    _i32FrameCnt = 0;

    _xposdatalist.clear();
    _yposlist[0].clear();
    _yposlist[1].clear();
    _yspeedlist[0].clear();
    _yspeedlist[1].clear();
    _xspeeddatalist.clear();
}
/**
* @brief 前馈使能/禁止
*
**/
void QPosTestWidget::onEnabelBtnClicked()
{
    if(_pEnabelBtn->text() == "开使能")
    {
        _pEnabelBtn->setText("关使能");
        emit signal_startTest(_i32AixsNo,0x1A,0x01,0x0);
    }
    else
    {
        _pEnabelBtn->setText("开使能");
        emit signal_startTest(_i32AixsNo,0x1B,0x0,0);
    }
}

/**
* @brief
*
**/
void QPosTestWidget::rcvFrameData(st_loopbackdata rcvdata)
{
    _rcvframedata = rcvdata;

    _lfPosSetdata = _rcvframedata.i32PosSetdata;
    _lfPosResponsedata = _rcvframedata.i32PosRespdata;

    _lfSpeedSetdata = _rcvframedata.i16SpeedSetdata;
    _lfSpeedResponsedata = _rcvframedata.i16SpeedRespdata;

    saveDataToFile();


    QString sInfo;
    sInfo = QString("当前位置:%1\n").arg(_lfPosResponsedata);
    _pShowPosInfoLabel->setText(sInfo);


    //添加数据
    if(_bStartTest == false)
    {
        return;
    }

    _xposdatalist.append(_i32FrameCnt*10);

    //位置
    _yposlist[0].append(_lfPosSetdata);
    _yposlist[1].append(_lfPosResponsedata);


    //速度
    _xspeeddatalist.append(_i32FrameCnt*20);
    _yspeedlist[0].append(_lfSpeedSetdata);
    _yspeedlist[1].append(_lfSpeedResponsedata);

    _i32FrameCnt++;
}

/**
* @brief
*
**/
void QPosTestWidget::plotPosData()
{
    if(_bStartTest == false)
    {
        return;
    }
    _pPosPlot->graph(0)->setData(_xposdatalist,_yposlist[0]);
    _pPosPlot->graph(1)->setData(_xposdatalist,_yposlist[1]);

    double lfxmax = *std::max_element(std::begin(_xposdatalist), std::end(_xposdatalist));
    double lfxmin = *std::min_element(std::begin(_xposdatalist), std::end(_xposdatalist));

    double lfymax0 = *std::max_element(std::begin(_yposlist[1]), std::end(_yposlist[1]));
    double lfymin0 = *std::min_element(std::begin(_yposlist[1]), std::end(_yposlist[1]));


    if((lfxmax) > 5000)
    {
        lfxmin = lfxmax - 5000;
    }

    if(lfxmax < 5000)
    {
        lfxmax = 5000;
    }
    _pPosPlot->xAxis->setRange((lfxmin),(lfxmax));//幅度数据
    _pPosPlot->yAxis->setRange(lfymin0*0.9,lfymax0*1.1);//幅度数据

    _pPosPlot->replot();
}
/**
* @brief
*
**/
void QPosTestWidget::plotSpeedData()
{
    if(_bStartTest == false)
    {
        return;
    }
    _pSpeedPlot->graph(0)->setData(_xspeeddatalist,_yspeedlist[0]);
    _pSpeedPlot->graph(1)->setData(_xspeeddatalist,_yspeedlist[1]);

    double lfxmax = *std::max_element(std::begin(_xspeeddatalist), std::end(_xspeeddatalist));
    double lfxmin = *std::min_element(std::begin(_xspeeddatalist), std::end(_xspeeddatalist));

    double lfymax0 = *std::max_element(std::begin(_yspeedlist[1]), std::end(_yspeedlist[1]));
    double lfymin0 = *std::min_element(std::begin(_yspeedlist[1]), std::end(_yspeedlist[1]));


    if((lfxmax) > 5000)
    {
        lfxmin = lfxmax - 5000;
    }

    if(lfxmax < 5000)
    {
        lfxmax = 5000;
    }
    _pSpeedPlot->xAxis->setRange((lfxmin),(lfxmax));//幅度数据
    _pSpeedPlot->yAxis->setRange(lfymin0*0.9,lfymax0*1.1);//幅度数据

    _pSpeedPlot->replot();
}
/**
* @brief
*
**/
void QPosTestWidget::setButtonStyleSheet(QPushButton* pBtn)
{
    pBtn->setStyleSheet(
                "QPushButton:checked { background-color: rgb(245,90,70);;color:white;}"
                "QPushButton:unchecked { background-color: rgb(200,200,200);;color:black;}"
                "QPushButton{background-color: rgb(200,200,200);padding:5px;border-radius:5px;color:black;"
                "font-size:18px;font-family: 'Microsoft YaHei';}");


//    pBtn->setSizePolicy(QSizePolicy::Expanding,QSizePolicy::Expanding);
    pBtn->setMinimumHeight(20);
}
/**
* @brief 调试参数设置
*
**/
void QPosTestWidget::onSetParamBtnClicked()
{
    float lfKP = _pKPEdit->text().toFloat()*10;
    float lfKI = _pKIEdit->text().toFloat()*10;
    float lfKD = _pKDEdit->text().toFloat()*100;

    emit signal_setpid(_i32AixsNo,0x2A,lfKP,lfKI,lfKD);//调试参数
}
/**
* @brief
*
**/
void QPosTestWidget::onModifyParamBtnClicked()
{
    float lfKP = _pKPEdit->text().toFloat()*10;
    float lfKI = _pKIEdit->text().toFloat()*10;
    float lfKD = _pKDEdit->text().toFloat()*100;


    emit signal_setpid(_i32AixsNo,0x4A,lfKP,lfKI,lfKD);//注入参数
}

/**
* @brief
*
**/
void QPosTestWidget::saveDataToFile()
{
    float lfCurrentA = _rcvframedata.i16CurrentA*0.1;
    float lfCurrentB = _rcvframedata.i16CurrentB*0.1;

    float lfCurrentQSet = _rcvframedata.i16QSetdata*0.1;
    float lfCurrentQResp = _rcvframedata.i16QRespdata*0.1;

    if(_bStartSaveData == false)
    {
        return;
    }

    _i32SaveFrameCnt++;

    int l_time = _i32SaveFrameCnt*10;

    QString sInfo;
    sInfo = QString("%1,%2,%3,%4,%5,%6,%7,%8\r").arg(l_time).arg(lfCurrentA).arg(lfCurrentB).\
            arg(_lfPosSetdata).arg(_lfPosResponsedata).arg(_lfSpeedSetdata).arg(_lfSpeedResponsedata,0,'f',6).arg(lfCurrentQSet,0,'f',6)\
            .arg(lfCurrentQResp,0,'f',6);

    m_sSaveDataInfo = m_sSaveDataInfo + sInfo;
    if(m_sSaveDataInfo.size() > 1000)
    {
        writeDataToFile(m_sSaveDataInfo);
        m_sSaveDataInfo = "";
    }

}


/**
* @brief
*
**/
void QPosTestWidget::onSaveDataBtnClick()
{
    if(_pSaveDataCheckBox->checkState() == Qt::Checked)
    {
        QString sFileFullPath = QDateTime::currentDateTime().toString("yyyyMMddhhmmss")+".csv";
        sFileFullPath = QApplication::applicationDirPath()+"/data/"+sFileFullPath;

        m_savefile.setFileName(sFileFullPath);

        writeDataToFile("时间,A相电流,B相电流,位置指令值,位置反馈值,速度指令,速度反馈,Q相电流指令(A),Q相电流反馈(A)\r");

        _i32SaveFrameCnt = 0;

        _bStartSaveData = true;

    }
    else
    {

        _bStartSaveData = false;

        writeDataToFile(m_sSaveDataInfo);
        m_sSaveDataInfo = "";
        m_savefile.close();
    }
}

void QPosTestWidget::writeDataToFile(QString str)
{
    if(m_savefile.fileName().isEmpty())
    {
        return;
    }

    if(!m_savefile.isOpen())
    {
        m_savefile.open(QFile::ReadWrite|QFile::Append);
    }

    m_savefile.write(str.toLocal8Bit());
}

/**
* @brief
*
**/
void QPosTestWidget::onShowPIDParam(int cmdid,float lfKP,float lfKI,float lfKD)
{
    if(cmdid != 0x8A)
    {
        return;
    }
    QString sInfo;
    sInfo = sInfo + QString("当前KP:%1\n").arg(lfKP);
    sInfo = sInfo + QString("当前KI:%1\n").arg(lfKI);
    sInfo = sInfo + QString("当前KD:%1\n").arg(lfKD);
    _pShowDataLabel->setText(sInfo);
}
